//
// Created by 于久 on 2024/12/12.
//

#include "hw_pid.h"

#define GW_GRAY_ADDR GW_GRAY_ADDR_DEF

extern tPid pidHW_Tracking; //红外循迹的PID
volatile uint8_t sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8;
volatile int ret_receive = 0;
volatile int ret = 0;
uint8_t recv_value = 0;
float g_cThisstate = 0; //这次状态
float g_cLaststate = 0; //上次状态
float g_fHW_PID_Out = 0; //红外对管PID计算输出速度
float g_fHW_PID_out1 = 0; //电机1的最后循迹PID控制速度
float g_fHW_PID_out2 = 0; //电机2的最后循迹PID控制速度
float g_fHW_PID_out1_back = 0; //电机1的最后循迹PID控制速度
float g_fHW_PID_out2_back = 0; //电机2的最后循迹PID控制速度
#if 0
void HW_GET() {
    ret_receive = HAL_I2C_Master_Receive(&hi2c1, GW_GRAY_ADDR << 1, &recv_value, 1, 100);
    if (ret_receive != HAL_OK) {
        // i2c 错误
    }
    SEP_ALL_BIT8(recv_value, sensor1, sensor2, sensor3, sensor4,
                 sensor5, sensor6, sensor7, sensor8);
    if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 && sensor7 == 1 &&
        sensor8 == 1) {
        g_cThisstate = 0;
        //Motor_Set_Speed(1.5, 1.5);
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 0) {
        g_cThisstate = -1;
        // Motor_Set_Speed(0.4, 1.5);
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 0 &&
               sensor8 == 0) {
        g_cThisstate = -0.8;
        //Motor_Set_Speed(0.5, 1.5);
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 0 &&
               sensor8 == 1) {
        g_cThisstate = -0.7;
        //Motor_Set_Speed(0.6, 1.1);
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 0 &&
               sensor7 == 0 &&
               sensor8 == 1) {
        g_cThisstate = -0.6;
        //Motor_Set_Speed(0.7, 1.0);
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 0 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = -0.5;
        //Motor_Set_Speed(0.8, 1.5);
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 0 && sensor6 == 0 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = -0.4;
        //Motor_Set_Speed(0.9, 1.5);
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 0 && sensor6 == 1 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = 0;
        //Motor_Set_Speed(1.1, 1.5);
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0 && sensor6 == 1 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = 0;
        //Motor_Set_Speed(1.5, 1.5);
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 0 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = 0;
        //Motor_Set_Speed(1.5, 1.1);
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 0 && sensor4 == 0 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = 0.4;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 0 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = 0.5;
    } else if (sensor1 == 1 && sensor2 == 0 && sensor3 == 0 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = 0.6;
    } else if (sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = 0.7;
        //Motor_Set_Speed(1.5, 0.6);
    } else if (sensor1 == 0 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = 0.8;
    } else if (sensor1 == 0 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 &&
               sensor8 == 1) {
        g_cThisstate = 1;
    }
#if 1
    g_fHW_PID_Out = PID_realize(&pidHW_Tracking, g_cThisstate);
    g_fHW_PID_out1 = 1 + (g_fHW_PID_Out);
    g_fHW_PID_out2 = 1 - (g_fHW_PID_Out);

    //printf(" %f %f %f \r\n", g_fHW_PID_Out, g_fHW_PID_out1, g_fHW_PID_out2);

    if (g_fHW_PID_out1 > 1) g_fHW_PID_out1 = 1;
    if (g_fHW_PID_out1 < 0) g_fHW_PID_out1 = 0;
    if (g_fHW_PID_out2 > 1) g_fHW_PID_out2 = 1;
    if (g_fHW_PID_out2 < 0) g_fHW_PID_out2 = 0;

    if (g_cThisstate != g_cLaststate) {
        Motor_Set_Speed(g_fHW_PID_Out, g_fHW_PID_Out);
    }
    g_cLaststate = g_cThisstate;
#endif
}

#endif

void HW_GET(void) {
    ret_receive = HAL_I2C_Master_Receive(&hi2c1, GW_GRAY_ADDR << 1, &recv_value, 1, 100);
    if (ret_receive != HAL_OK) {
        // i2c 错误
    }
    SEP_ALL_BIT8(recv_value, sensor1, sensor2, sensor3, sensor4, sensor5, sensor6, sensor7, sensor8);
    if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 && sensor7 == 1 &&
        sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 0) {
        g_fHW_PID_out1 = 0.4;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 0 && sensor8 == 0) {
        g_fHW_PID_out1 = 0.6;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 0 && sensor8 == 1) {
        g_fHW_PID_out1 = 0.8;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 0 &&
               sensor7 == 0 && sensor8 == 1) {
        g_fHW_PID_out1 = 1;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 0 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.2;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 0 && sensor6 == 0 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.4;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 0 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.6;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 0 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 1.6;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 0 && sensor4 == 0 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 1.4;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 0 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 1.2;
    } else if (sensor1 == 1 && sensor2 == 0 && sensor3 == 0 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 1;
    } else if (sensor1 == 1 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 0.8;
    } else if (sensor1 == 0 && sensor2 == 0 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 0.6;
    } else if (sensor1 == 0 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 0.4;
    } else if (sensor1 == 0 && sensor2 == 0 && sensor3 == 0 && sensor4 == 1 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 0;
    } else if (sensor1 == 1 && sensor2 == 0 && sensor3 == 0 && sensor4 == 0 && sensor5 == 1 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.8;
        g_fHW_PID_out2 = 0.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 1 && sensor6 == 0 &&
               sensor7 == 0 && sensor8 == 0) {
        g_fHW_PID_out1 = 0;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 1 && sensor5 == 0 && sensor6 == 0 &&
               sensor7 == 0 && sensor8 == 1) {
        g_fHW_PID_out1 = 0.8;
        g_fHW_PID_out2 = 1.8;
    } else if (sensor1 == 0 && sensor2 == 0 && sensor3 == 0 && sensor4 == 0 && sensor5 == 0 && sensor6 == 0 &&
               sensor7 == 0 && sensor8 == 0) {
        g_fHW_PID_out1 = 0;
        g_fHW_PID_out2 = 0;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 1 && sensor4 == 0 && sensor5 == 0 && sensor6 == 0 &&
               sensor7 == 0 && sensor8 == 0) {
        g_fHW_PID_out1 = 0;
        g_fHW_PID_out2 = 1.6;
    } else if (sensor1 == 0 && sensor2 == 0 && sensor3 == 0 && sensor4 == 0 && sensor5 == 0 && sensor6 == 1 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.6;
        g_fHW_PID_out2 = 0;
    } else if (sensor1 == 1 && sensor2 == 1 && sensor3 == 0 && sensor4 == 0 && sensor5 == 0 && sensor6 == 0 &&
               sensor7 == 0 && sensor8 == 0) {
        g_fHW_PID_out1 = 0;
        g_fHW_PID_out2 = 1.6;
    } else if (sensor1 == 0 && sensor2 == 0 && sensor3 == 0 && sensor4 == 0 && sensor5 == 0 && sensor6 == 0 &&
               sensor7 == 1 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.6;
        g_fHW_PID_out2 = 0;
    } else if (sensor1 == 1 && sensor2 == 0 && sensor3 == 0 && sensor4 == 0 && sensor5 == 0 && sensor6 == 0 &&
               sensor7 == 0 && sensor8 == 0) {
        g_fHW_PID_out1 = 0;
        g_fHW_PID_out2 = 1.6;
    } else if (sensor1 == 0 && sensor2 == 0 && sensor3 == 0 && sensor4 == 0 && sensor5 == 0 && sensor6 == 0 &&
               sensor7 == 0 && sensor8 == 1) {
        g_fHW_PID_out1 = 1.6;
        g_fHW_PID_out2 = 0;
    }

    if (g_fHW_PID_out1_back != g_fHW_PID_out1 || g_fHW_PID_out2 != g_fHW_PID_out2_back) {
        Motor_Set_Speed(g_fHW_PID_out1, g_fHW_PID_out2);
    }

    g_fHW_PID_out1_back = g_fHW_PID_out1;
    g_fHW_PID_out2_back = g_fHW_PID_out2;
}

